% Task 4: Fit lines to data points, using total least squares and 
% RANSAC + total least squares

% Clear up
clc;
close all;
clearvars;

% Begin by loading data points from linedata.mat
load linedata

N = length(xm); % number of data points

% Plot data
plot(xm, ym, '*'); hold on;
xlabel('x') 
ylabel('y')
title('TASK 4') % OBS - CHANGE TITLE!
x_fine = [min(xm)-0.1,max(xm)+0.1]; % used when plotting the fitted lines

% Fit a line to these data points with total least squares
% Here you should write code to obtain the p_ls coefficients (assuming the
% line has the form y = p_ls(1) * x + p_ls(2)).
% Least squares fitting
X = [xm ones(N,1)];
p_ls = (X'*X)\(X'*ym);
plot(x_fine, p_ls(1) * x_fine + p_ls(2), 'r')

X_tls = [xm,ym, ones(N, 1)];

% Total least squares fitting
% Construct the covariance matrix
A = [sum(xm.^2) - (1/length(xm)) * (sum(xm))^2, sum(xm.*ym) - (1/length(xm)) * (sum(xm)) * (sum(ym)); 
    sum(xm.*ym) - (1/length(xm)) * (sum(xm)) * (sum(ym)), sum(ym.^2) - (1/length(xm)) * (sum(ym))^2];

% Compute engVal and eigenvectors
[V, D] = eig(A);

% Extract engVal and the first engVec
engVal = diag(D);
engVec = V(:, 1); % Select the first engVec

% Normalize the engVec to make A^2 + B^2 = 1
engVec = engVec / sqrt(engVec(1)^2 + engVec(2)^2);

% Extract coefficients A, B, and C of the line equation Ax + By + C = 0
a = engVec(1);
b = engVec(2);
c = -(a * mean(xm) + b * mean(ym));

% Output the line equation
fprintf('Fitted line equation: %.2f*x + %.2f*y + %.2f = 0\n', a, b, c);

% Calculate p and c for the TLS line equation
p_tls = -a/b;
c_tls = -c/b;

% Plot the TLS line
plot(x_fine, p_tls * x_fine + c_tls, 'b');
plot(x_fine, p_tls * x_fine + c_tls, 'b');


% RANSAC fitting
iterNum = 2000;
threshDist = 0.01;
inlierRatio = 0.1;

% Initialize the best fitting line
bestInNum = 0;  % Best fitting line with largest number of inliers
bestParameter1=0; bestParameter2=0;  % parameters for best fitting line

for i=1:iterNum
    % Randomly select 2 points
    idx = randperm(N,2);
    sampleX = xm(idx);
    sampleY = ym(idx);
    
    % Compute the distances between all points with the fitting line 
    kLine = (sampleY(2)-sampleY(1))/(sampleX(2)-sampleX(1));  % slope
    dLine = sampleY(1)-kLine*sampleX(1);  % intercept
    
    dists = abs(kLine*xm-ym+dLine)/sqrt(kLine^2+1);
    
    % Compute the inliers with distances smaller than the threshold
    inlierIdx = find(dists<=threshDist);
    inlierNum = length(inlierIdx);
    
    % Update the number of inliers and fitting model if better model is found     
    if inlierNum>=round(inlierRatio*N) && inlierNum>bestInNum
        bestInNum = inlierNum;
        bestParameter1=kLine; bestParameter2=dLine;
    end
end

disp(bestInNum);

% Plot the best fitting line
plot(x_fine, bestParameter1*x_fine + bestParameter2, 'g')

% Legend --> show which line corresponds to what (if you need to
% re-position the legend, you can modify rect below)
h=legend('data points', 'least-squares', 'Total least-squares','RANSAC');
rect = [0.20, 0.65, 0.25, 0.25];
set(h, 'Position', rect)

% After having plotted both lines, it's time to compute errors for the
% respective lines. Specifically, for each line (the total least squares and the
% RANSAC line), compute the least square error and the total
% least square error. For the RANSAC solution compute errors on inlier set. 
% Note that the error is the sum of the individual
% squared errors for each data point! In total you should get 4 errors. Report these
% in your report, and comment on the results. OBS: Recall the distance formula
% between a point and a line from linear algebra, useful when computing orthogonal
% errors!

% WRITE CODE BELOW TO COMPUTE THE 4 ERRORS

% Compute the least square error for the total least squares line
ls_error_tls = sum((ym - (p_tls * xm + c_tls)).^2);
disp(['Least square error for the total least squares line: ', num2str(ls_error_tls)]);

% Compute the total least square error for the total least squares line
tls_error_tls = sum((xm - ((ym - c_tls) / p_tls)).^2);
disp(['Total least square error for the total least squares line: ', num2str(tls_error_tls)])

% Compute the least square error for the RANSAC line
ls_error_ransac = sum((ym(bestInNum) - (bestParameter1 * xm(bestInNum) + bestParameter2)).^2);
disp(['Least square error for the RANSAC line: ', num2str(ls_error_ransac)])

% Compute the total least square error for the RANSAC line
tls_error_ransac = sum((xm(bestInNum) - ((ym(bestInNum) - bestParameter2) / bestParameter1)).^2);
disp(['Total least square error for the RANSAC line: ', num2str(tls_error_ransac)])

% 
% % 计算 LS 模型的误差
% residuals_ls = ym - (p_ls(1) * xm + p_ls(2));
% error_ls = sum(residuals_ls .^ 2);
% disp(['Least Squares Error: ', num2str(error_ls)])
% 
% % 计算 TLS 模型的误差
% residuals_tls = ym - (p_tls * xm + c_tls);
% error_tls = sum(residuals_tls .^ 2);
% disp(['Total Least Squares Error: ', num2str(error_tls)])
% 
% % 计算 RANSAC 模型的误差
% residuals_ransac = ym - (bestParameter1 * xm + bestParameter2);
% error_ransac = sum(residuals_ransac .^ 2);
% disp(['RANSAC Error: ', num2str(error_ransac)])
% 
% % 计算 TLS 模型的 TLS 误差
% residuals_tls_tls = abs(p_tls * xm - ym + c_tls) / sqrt(p_tls^2 + 1);
% error_tls_tls = sum(residuals_tls_tls .^ 2);
% disp(['Total Least Squares Error (TLS): ', num2str(error_tls_tls)])
% 
% % 计算 RANSAC 模型的 TLS 误差
% residuals_ransac_tls = abs(bestParameter1 * xm - ym + bestParameter2) / sqrt(bestParameter1^2 + 1);
% error_ransac_tls = sum(residuals_ransac_tls .^ 2);
% disp(['RANSAC Error (TLS): ', num2str(error_ransac_tls)])